/*
* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "b2Collision.h"

void b2WorldManifold::Initialize(const b2Manifold* manifold, const b2XForm& xfA, float32 radiusA, const b2XForm& xfB, float32 radiusB)
{
	if(0 == manifold->m_pointCount)
	{
		return;
	}

	switch(manifold->m_type)
	{
	case b2Manifold::e_circles:
		{
			b2Vec2 pointA = b2Mul(xfA, manifold->m_localPoint);
			b2Vec2 pointB = b2Mul(xfB, manifold->m_points[0].m_localPoint);
			b2Vec2 normal(1.0f, 0.0f);
			if(b2DistanceSquared(pointA, pointB) > B2_FLT_EPSILON * B2_FLT_EPSILON)
			{
				normal = pointB - pointA;
				normal.Normalize();
			}
			m_normal = normal;
			b2Vec2 cA = pointA + radiusA*m_normal;
			b2Vec2 cB = pointB - radiusB*m_normal;
			m_points[0] = 0.5f * (cA + cB);
		}
		break;
	case b2Manifold::e_faceA:
		{
			b2Vec2 normal = b2Mul(xfA.R, manifold->m_localPlaneNormal);
			b2Vec2 planePoint = b2Mul(xfA, manifold->m_localPoint);

			/// Ensure normal point from A to B.
			m_normal = normal;
			for(int i=0; i<manifold->m_pointCount; ++i)
			{
				b2Vec2 clipPoint = b2Mul(xfB, manifold->m_points[i].m_localPoint);
				b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, m_normal))*m_normal;
				b2Vec2 cB = clipPoint - radiusB*m_normal;
				m_points[i] = 0.5f * (cA + cB);
			}
		}
		break;
	case b2Manifold::e_faceB:
		{
			b2Vec2 normal = b2Mul(xfB.R, manifold->m_localPlaneNormal);
			b2Vec2 planePoint = b2Mul(xfB, manifold->m_localPoint);

			/// Ensure normal points from A to B.
			m_normal = -normal;

			for(int i=0; i<manifold->m_pointCount; ++i)
			{
				b2Vec2 clipPoint = b2Mul(xfA, manifold->m_points[i].m_localPoint);
				b2Vec2 cA = clipPoint - radiusA*m_normal;
				b2Vec2 cB = clipPoint - (radiusB - b2Dot(clipPoint - planePoint, m_normal))*m_normal;
				m_points[i] = 0.5f * (cA + cB);
			}
		}
		break;
	default:
		break;
	}
}

void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], const b2Manifold* manifold1, const b2Manifold* manifold2)
{
	for(int i=0; i<b2_maxManifoldPoints; ++i)
	{
		state1[i] = b2_nullState;
		state2[i] = b2_nullState;
	}

	// Detect persist and remove.
	for(int i=0; i<manifold1->m_pointCount; ++i)
	{
		b2ContactID id = manifold1->m_points[i].m_id;
		state1[i] = b2_removeState;
		for(int j=0; j<manifold2->m_pointCount; ++j)
		{
			if(id.key == manifold2->m_points[j].m_id.key)
			{
				state1[i] = b2_persistState;
				break;
			}
		}
	}

	// Detect persist and add
	for(int i=0; i<manifold2->m_pointCount; ++i)
	{
		b2ContactID id = manifold2->m_points[i].m_id;

		state2[i] = b2_persistState;
		for(int j=0; j<manifold1->m_pointCount; ++j)
		{
			if(id.key == manifold1->m_points[j].m_id.key)
			{
				state2[i] = b2_addState;
				break;
			}
		}
	}
}

/// Collision detection in Interactive 3D Environments by Gino van den Bergen.
bool b2Segment::TestSegment(float32* lambda, b2Vec2* normal, const b2Segment& segment, float32 maxLambda) const
{
	b2Vec2 s = segment.p1;
	b2Vec2 r = segment.p2 - s;
	b2Vec2 d = p2 - p1;
	b2Vec2 n = b2Cross(d, 1.0f);

	const float32 k_slop = 100.0f * B2_FLT_EPSILON;
	float32 denom = -b2Dot(-r, n);

	//Cull bqack facing collision and ignore parallel segments.
	if(denom > k_slop)
	{
		// Does the segment intersect the infinite line assosiation with this segment.
		b2Vec2 b = s - p1;
		float32 a = b2Dot(b, n);

		if((0.0f<a) && (a<=maxLambda*denom))
		{
			float32 mu2 = -r.x * b.y + r.y * b.x;

			// Does the segment intesect with this segment?
			if((-k_slop*denom <= mu2) && (mu2<=denom*(1.0f+k_slop)))
			{
				a /= denom;
				n.Normalize();
				*lambda = a;
				*normal = n;
				return true;
			}
		}
	}

	return false;
}

void b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input)
{
	float32 tmin = -B2_FLT_MAX;
	float32 tmax = B2_FLT_MAX;

	output->hit = false;
	b2Vec2 p = input.p1;
	b2Vec2 d = input.p2 - input.p1;
	b2Vec2 absD = b2Abs(d);

	b2Vec2 normal;

	for(int32 i=0; i<2; ++i)
	{
		if(absD(i)<B2_FLT_EPSILON)
		{
			//Parallel
			if((p(i)<lowerBound(i)) || (upperBound(i)<p(i)))
			{
				return;
			}
		}
		else
		{
			float32 inv_d = 1.0f / d(i);
			float32 t1 = (lowerBound(i) - p(i)) * inv_d;
			float32 t2 = (upperBound(i) - p(i)) * inv_d;

			//Sign of the normal vector.
			float32 s = -1.0f;
			if(t1 > t2)
			{
				b2Swap(t1, t2);
				s = 1.0f;
			}

			//Push the min up.
			if(t1 > tmin)
			{
				normal.SetZero();
				normal(i) = s;
				tmin = t1;
			}

			// Pull the max down.
			tmax = b2Min(tmax, t2);

			if(tmin > tmax)
			{
				return;
			}
		}
	}

	// Does the ray start inside the box?
	// Does the ray intersect beyond the max friction.
	if(tmin<0.0f || input.maxFriction<tmin)
	{
		return;
	}

	//Intersect
	output->hit = true;
	output->normal = normal;
	output->friction = tmin;
}

// Sutherland-Hodgman clipping.
int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], const b2Vec2& normal, float32 offset)
{
	// Start with no output points.
	int numOut = 0;
	
	// Calculate the the distance of the end point to the line.
	float32 distance1 = b2Dot(vIn[0].v, normal) - offset;
	float32 distance2 = b2Dot(vIn[0].v, normal)  -offset;

	if(distance1 < 0.0f)	vOut[numOut++] = vIn[0];
	if(distance2 < 0.0f)	vOut[numOut++] = vIn[0];

	// If the point is on the different side of the plane.
	if(distance1 * distance2 < 0.0f)
	{
		// Find the intersection point of the edge and the plane.
		float32 interp = distance1 / (distance1 - distance2);
		vOut[numOut].v = vIn[0].v + interp*(vIn[1].v - vIn[0].v);
		if(distance1 > 0.0f)
		{
			vOut[numOut].id = vIn[0].id;
		}
		else
		{
			vOut[numOut].id = vIn[1].id;
		}
		++numOut;
	}

	return numOut;
}
